The steps of the project are the following and below are the pipeline stages :
Steps : Compute the camera calibration matrix and distortion coefficients given a set of chessboard images and undistort the image.
a. Open CV function calibrateCamera is used to find the camera matrix to calibrate the camera
b. The chess board images in camera_cal folder is used for calibration (9x6 points)
import numpy as np
import cv2
import matplotlib.pyplot as plt
import matplotlib.image as mpimg
%matplotlib inline
import glob
import pickle
# prepare object points
nx = 9#TODO: enter the number of inside corners in x
ny = 6#TODO: enter the number of inside corners in y
# Initialise image and object point arrays
objpoints = [] # this is the 3d domain pts
imgpoints = [] # this the distorted points in 2d domain
# Generate object points
objp = np.zeros((nx*ny,3), np.float32)
objp[:,:2] = np.mgrid[0:nx,0:ny].T.reshape(-1,2) # x, y coordinates
def cal_undistort(img, objpoints, imgpoints):
# Calibrate camera
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, img.shape[0:2], None, None)
# Undistort image
undist = cv2.undistort(img, mtx, dist, None, mtx)
return undist
#Read Camera calibration images
images = glob.glob("camera_cal/calibration*.jpg")
for fname in images :
img = cv2.imread(fname)
#plt.imshow(img)
# Convert to grayscale
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Find the chessboard corners
ret, corners = cv2.findChessboardCorners(gray, (nx, ny), None)
#print(ret)
# If found, draw corners
if ret == True:
imgpoints.append(corners)
objpoints.append(objp)
# Draw and display the corners
cv2.drawChessboardCorners(img, (nx, ny), corners, ret)
plt.imshow(img)
import pickle
#calibrate camera and save camera matrix as pickle file
img_size = (img.shape[1], img.shape[0])
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, img_size,None,None)
# Save the camera calibration result for later use (we won't worry about rvecs / tvecs)
cam_pickle = {}
cam_pickle["mtx"] = mtx
cam_pickle["dist"] = dist
pickle.dump( cam_pickle, open( "camera_matrix.p", "wb" ) )
Here are the original image and undistored images of chess board after applying camera calibraion.
# Test undistortion on an image
img = cv2.imread('camera_cal/calibration1.jpg')
img_size = (img.shape[1], img.shape[0])
# Do camera calibration given object points and image points
out = cv2.undistort(img, mtx, dist, None, mtx)
cv2.imwrite('output_images/orig_calibration1.jpg',img)
cv2.imwrite('output_images/undist_calibration1.jpg',out)
f, (im1, im2) = plt.subplots(1, 2, figsize=(20,10))
im1.imshow(img)
im1.set_title('Original Image', fontsize=20)
im2.imshow(out)
im2.set_title('Undistorted Image', fontsize=20)
Read Image and apply distortion correction Below are sample images from test_images before and after undistortion.
import numpy as np
import cv2
import matplotlib.pyplot as plt
import matplotlib.image as mpimg
import pickle
%matplotlib inline
with open("camera_matrix.p", mode='rb') as f:
cam_pickle = pickle.load(f)
mtx = cam_pickle["mtx"]
dist = cam_pickle["dist"]
read_img = cv2.imread("test_images/test2.jpg")
#read_image = mpimg.imread("test_images/test1.jpg")
#since opencv reads image in bgr format
image_corr=cv2.cvtColor(read_img,cv2.COLOR_BGR2RGB)
undistorted_input = cv2.undistort(image_corr, mtx, dist, None, mtx)
cv2.imwrite('output_images/undist_test1.jpg',undistorted_input)
imshape = image_corr.shape
# Visualize undistortion
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(50,50))
ax1.imshow(image_corr)
ax1.set_title('Original Image', fontsize=20)
ax2.imshow(undistorted_input)
ax2.set_title('Undistorted Image', fontsize=20)
Apply Gradients, thresholding and Color Transforms (S-Domain) for Binary Image. See transform_pipeline function to see how these are applied.
from primitives import *
sx_thresh = (30,100)
s_thresh =(160,255)
combined_binary = transform_pipeline(undistorted_input, s_thresh, sx_thresh )
plt.imshow(combined_binary, cmap="gray")
cv2.imwrite('output_images/grad_test1.jpg',combined_binary)
Below is the image with only the region of interest
vertices = np.array([[(0,imshape[0]),(650, 430), (730, 430), (imshape[1],imshape[0])]], dtype=np.int32)
masked_image = region_of_interest(combined_binary, vertices)
plt.imshow(masked_image, cmap="gray")
Calculate the perspective tranform factors. The src and dst vectors were obtained by manually reviewing the test images provided. Below is the image with perspective transform applied on the above binary image.
src = np.float32(
[[300, 680],
[600, 460],
[730, 460],
[1080, 680]])
dst = np.float32(
[[300,680],
[300,0],
[1000,0],
[1080,680]])
M = cv2.getPerspectiveTransform(src, dst)
Minv = cv2.getPerspectiveTransform(dst, src)
warped_img = cv2.warpPerspective(combined_binary, M, (imshape[1], imshape[0]), flags=cv2.INTER_LINEAR)
plt.imshow(warped_img, cmap="gray")
cv2.imwrite('output_images/warped_img_test1.jpg',warped_img)
import numpy as np
#histogram = np.sum(warped_img[warped_img.shape[0]//2:,:], axis=0)
#plt.plot(histogram)
Fit curves for lane markings based on the warped binary image from above. This is the same pipeline used in class.
import numpy as np
import cv2
import matplotlib.pyplot as plt
# Assuming you have created a warped binary image called "binary_warped"
# Take a histogram of the bottom half of the image
def find_lane(binary_warped, draw=True):
histogram = np.sum(binary_warped[binary_warped.shape[0]//2:,:], axis=0)
# Create an output image to draw on and visualize the result
out_img = np.dstack((binary_warped, binary_warped, binary_warped))*255
# Find the peak of the left and right halves of the histogram
# These will be the starting point for the left and right lines
midpoint = np.int(histogram.shape[0]/2)
leftx_base = np.argmax(histogram[:midpoint])
rightx_base = np.argmax(histogram[midpoint:]) + midpoint
# Choose the number of sliding windows
nwindows = 9
# Set height of windows
window_height = np.int(binary_warped.shape[0]/nwindows)
# Identify the x and y positions of all nonzero pixels in the image
nonzero = binary_warped.nonzero()
nonzeroy = np.array(nonzero[0])
nonzerox = np.array(nonzero[1])
# Current positions to be updated for each window
leftx_current = leftx_base
rightx_current = rightx_base
# Set the width of the windows +/- margin
margin = 100
# Set minimum number of pixels found to recenter window
minpix = 50
# Create empty lists to receive left and right lane pixel indices
left_lane_inds = []
right_lane_inds = []
# Step through the windows one by one
for window in range(nwindows):
# Identify window boundaries in x and y (and right and left)
win_y_low = binary_warped.shape[0] - (window+1)*window_height
win_y_high = binary_warped.shape[0] - window*window_height
win_xleft_low = leftx_current - margin
win_xleft_high = leftx_current + margin
win_xright_low = rightx_current - margin
win_xright_high = rightx_current + margin
# Draw the windows on the visualization image
cv2.rectangle(out_img,(win_xleft_low,win_y_low),(win_xleft_high,win_y_high),(0,255,0), 2)
cv2.rectangle(out_img,(win_xright_low,win_y_low),(win_xright_high,win_y_high),(0,255,0), 2)
# Identify the nonzero pixels in x and y within the window
good_left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & (nonzerox >= win_xleft_low) & (nonzerox < win_xleft_high)).nonzero()[0]
good_right_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & (nonzerox >= win_xright_low) & (nonzerox < win_xright_high)).nonzero()[0]
# Append these indices to the lists
left_lane_inds.append(good_left_inds)
right_lane_inds.append(good_right_inds)
# If you found > minpix pixels, recenter next window on their mean position
if len(good_left_inds) > minpix:
leftx_current = np.int(np.mean(nonzerox[good_left_inds]))
if len(good_right_inds) > minpix:
rightx_current = np.int(np.mean(nonzerox[good_right_inds]))
# Concatenate the arrays of indices
left_lane_inds = np.concatenate(left_lane_inds)
right_lane_inds = np.concatenate(right_lane_inds)
# Extract left and right line pixel positions
leftx = nonzerox[left_lane_inds]
lefty = nonzeroy[left_lane_inds]
rightx = nonzerox[right_lane_inds]
righty = nonzeroy[right_lane_inds]
# Fit a second order polynomial to each
left_fit = np.polyfit(lefty, leftx, 2)
right_fit = np.polyfit(righty, rightx, 2)
ploty = np.linspace(0, binary_warped.shape[0]-1, binary_warped.shape[0] )
left_fitx = left_fit[0]*ploty**2 + left_fit[1]*ploty + left_fit[2]
right_fitx = right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2]
out_img[nonzeroy[left_lane_inds], nonzerox[left_lane_inds]] = [255, 0, 0]
out_img[nonzeroy[right_lane_inds], nonzerox[right_lane_inds]] = [0, 0, 255]
if(draw) :
plt.imshow(out_img)
plt.plot(left_fitx, ploty, color='yellow')
plt.plot(right_fitx, ploty, color='yellow')
plt.xlim(0, 1280)
plt.ylim(720, 0)
cv2.imwrite('output_images/lane_markings_test1.jpg',out_img)
return leftx, lefty, rightx, righty, left_fit , right_fit, out_img , ploty, left_fitx, right_fitx
binary_warped = np.zeros_like(warped_img[:,:])
binary_warped[(warped_img[:,:] > 0)] = 1
leftx, lefty, rightx, righty, left_fit , right_fit , lane_img , ploty, left_fitx, right_fitx = find_lane(binary_warped)
print(righty)
print(left_fit)
print(right_fit)
Define Functions for calculating curvature and offset. The radius of curvature is transformed from the pixel based y_to_fit and x_to_fit params to meters and a new polynomal is fit for this. The road curvature is assumed to be circular.
For the position of the vehicle, the camera is assumed to be mounted at the center of the car and the deviation of the midpoint of the lane from the center of the image is the output offset. It is assumed that lane_width is 3.7m and pixel wise x and y values are converted to meter scale.
Below is the image with Unwarped lane markings in the orignal image along with curvature and lane offset statistics embedded.
def calc_curvature(y_to_fit, x_to_fit, y_eval):
# Conversion factors for pixels to meters
m_per_pix_y, m_per_pix_x = 30/720, 3.7/700 # taken from class
# Fit a new polynomial to world-space (in meters)
fit = np.polyfit(y_to_fit*m_per_pix_y, x_to_fit*m_per_pix_x, 2)
curvature = ((1 + (2*fit[0]*(y_eval*m_per_pix_y) + fit[1])**2)**1.5) / np.absolute(2*fit[0])
return curvature
def calc_offset(left_x, right_x, img_center_x):
lane_width = abs(left_x - right_x)
lane_center_x = (left_x + right_x)//2
pix_offset = img_center_x - lane_center_x
lane_width_m = 3.7
return lane_width_m * (pix_offset/lane_width)
left_curv = calc_curvature(lefty, leftx, np.max(ploty))
right_curv = calc_curvature(righty, rightx, np.max(ploty))
print(left_curv)
print(right_curv)
offset = calc_offset(left_fitx[-1], right_fitx[-1], binary_warped.shape[1]//2)
print(offset)
#Overlap orig image with lane polygon
warp_zero = np.zeros_like(binary_warped).astype(np.uint8)
color_warp = np.dstack((warp_zero, warp_zero, warp_zero))
pts_left = np.array([np.transpose(np.vstack([left_fitx, ploty]))])
pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx, ploty])))])
pts = np.hstack((pts_left, pts_right))
# Draw the lane onto the warped blank image
cv2.fillPoly(color_warp, np.int_([pts]), (0,255, 0))
newwarp = cv2.warpPerspective(color_warp, Minv, (undistorted_input.shape[1], undistorted_input.shape[0]))
# Combine the result with the original image
result = cv2.addWeighted(undistorted_input, 1, newwarp, 0.3, 0)
cv2.imwrite('output_images/final_lane_polygon_test1.jpg',result)
cv2.putText(result,'Radius of Left Curvature: %.2fm' % left_curv,(20,40), cv2.FONT_HERSHEY_SIMPLEX, 1,(255,255,255),2)
cv2.putText(result,'Radius of Right Curvature: %.2fm' % right_curv,(20,80), cv2.FONT_HERSHEY_SIMPLEX, 1,(255,255,255),2)
cv2.putText(result,'Center Lane Offset: %.2fm' % offset,(20,120), cv2.FONT_HERSHEY_SIMPLEX, 1,(255,255,255), 2)
plt.imshow(result)
Consolidate the entire image pipeline described above to a single function to be used for video processing. Additional metrics added : We save the previous frame's lane markings and maintain the lane markings from previous frame if the radius of curvature is predicted to be below a threshold of 460 m and the offset is less than -1.5 or greater than 1.5 m. This is to prevent cases where the pipeline predicts certain lane markings to be wrong due to shadows or changes in road texture, and this transform seems to fit well for the test video.
## Full Image pipeline
import numpy as np
import cv2
import matplotlib.pyplot as plt
import matplotlib.image as mpimg
import pickle
%matplotlib inline
with open("camera_matrix.p", mode='rb') as f:
cam_pickle = pickle.load(f)
mtx = cam_pickle["mtx"]
dist = cam_pickle["dist"]
def keep_last_fit(left_curve, right_curve,offset):
if right_curve < 460 or left_curve < 460 or offset > 1.5 or offset < -1.5:
return False
else:
return True
last_left_fit = None
last_right_fit = None
last_left_fitx = None
last_right_fitx = None
last_ploty = None
def image_pipeline(input_file, filepath=False):
global last_left_fit
global last_right_fit
global last_left_fitx
global last_right_fitx
global last_ploty
plt.clf()
if filepath == True:
# Read in image
read_img = cv2.imread(input_file)
else:
read_img = input_file
#image_corr=cv2.cvtColor(read_img,cv2.COLOR_BGR2BGR)
undistorted_input = cv2.undistort(read_img, mtx, dist, None, mtx)
imshape = image_corr.shape
#### Edge Detection
sx_thresh = (30,100)
s_thresh =(160,255)
combined_binary = transform_pipeline(undistorted_input, s_thresh, sx_thresh )
######## Apply perspective transform
src = np.float32(
[[300, 680],
[600, 460],
[730, 460],
[1080, 680]])
dst = np.float32(
[[300,680],
[300,0],
[1000,0],
[1080,680]])
M = cv2.getPerspectiveTransform(src, dst)
Minv = cv2.getPerspectiveTransform(dst, src)
warped_img = cv2.warpPerspective(combined_binary, M, (imshape[1], imshape[0]), flags=cv2.INTER_LINEAR)
####### find lane
binary_warped = np.zeros_like(warped_img[:,:])
binary_warped[(warped_img[:,:] > 0)] = 1
leftx, lefty, rightx, righty, left_fit , right_fit , lane_img , ploty, left_fitx, right_fitx = find_lane(binary_warped,False)
###### calc curvatur and curv
left_curv = calc_curvature(lefty, leftx, np.max(ploty))
right_curv = calc_curvature(righty, rightx, np.max(ploty))
print(left_curv)
print(right_curv)
offset = calc_offset(left_fitx[-1], right_fitx[-1], binary_warped.shape[1]//2)
print(offset)
if(keep_last_fit(left_curv,right_curv,offset)) :
last_left_fitx = left_fitx
last_right_fitx = right_fitx
last_ploty = ploty
else :
left_fitx = last_left_fitx
right_fitx = last_right_fitx
ploty = last_ploty
#Overlap orig image with lane polygon
warp_zero = np.zeros_like(binary_warped).astype(np.uint8)
color_warp = np.dstack((warp_zero, warp_zero, warp_zero))
pts_left = np.array([np.transpose(np.vstack([left_fitx, ploty]))])
pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx, ploty])))])
pts = np.hstack((pts_left, pts_right))
# Draw the lane onto the warped blank image
cv2.fillPoly(color_warp, np.int_([pts]), (0,255, 0))
newwarp = cv2.warpPerspective(color_warp, Minv, (undistorted_input.shape[1], undistorted_input.shape[0]))
# Combine the result with the original image
result = cv2.addWeighted(undistorted_input, 1, newwarp, 0.3, 0)
cv2.putText(result,'Radius of Left Curvature: %.2fm' % left_curv,(20,40), cv2.FONT_HERSHEY_SIMPLEX, 1,(255,255,255),2)
cv2.putText(result,'Radius of Right Curvature: %.2fm' % right_curv,(20,80), cv2.FONT_HERSHEY_SIMPLEX, 1,(255,255,255),2)
cv2.putText(result,'Center Lane Offset: %.2fm' % offset,(20,120), cv2.FONT_HERSHEY_SIMPLEX, 1,(255,255,255), 2)
plt.imshow(result)
cv2.imwrite('output_images/final_lane_markings_all_pipe_test1.jpg',result)
return result
combined_img = image_pipeline(read_img)
Test the pipeline on video
from moviepy.editor import VideoFileClip
from IPython.display import HTML
output = 'project_output_colour.mp4'
clip1 = VideoFileClip("project_video.mp4")
output_clip = clip1.fl_image(image_pipeline) #NOTE: this function expects color images!!
%time output_clip.write_videofile(output, audio=False)
Discussion
I had to spend some time to figure out the thresold for the gradient transformation and the src,dest functions for perspective transform. The test case worked only upon fine tuning these.
The pipeline doesnt perform very well in the challenge test case. I think the transform to keep the lane markings from previous stage for radius/offset under a certain threshold is not a very reliable mechanism. The pipeline needs to be fixed to be more accurate, I guess more time needs to be invested in thresholding and gradient part of the pipeline to fix this.
The pipeline wouldnt work well if the car deviates from the center of lane or does a lane change